/******************************************************************************
 * Copyright 2022 The AIR Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#include <cstdlib>
#include <iostream>
#include <string>
#include <vector>

#include "MessageFrame.h"
#include "v2x-asn-msgs-internal.h"

bool miscs_xml_node2long(const pugi::xml_node &xml, int64_t *res,
                         bool required = false) {
  if (xml.empty() || !res) {
    return !required;
  }
  std::string str(xml.child_value());
  if (str.empty()) {
    return false;
  }
  char *tail = nullptr;
  auto result = strtol(str.c_str(), &tail, 10);
  if (str.length() != tail - str.c_str()) {
    LOG(INFO) << "\"" << str << "\" is NOT a integer!";
    return false;
  }
  *res = result;
  return true;
}

bool miscs_xml_node2double(const pugi::xml_node &xml, double *res,
                           bool required = false) {
  if (xml.empty() || !res) {
    return !required;
  }
  std::string str(xml.child_value());
  if (str.empty()) {
    return false;
  }
  char *tail = nullptr;
  auto result = strtod(str.c_str(), &tail);
  if (str.length() != tail - str.c_str()) {
    LOG(INFO) << "\"" << str << "\" is NOT a double!";
    return false;
  }
  *res = result;
  return true;
}

bool miscs_xml_node2string(const pugi::xml_node &xml, std::string *res,
                           bool required = false) {
  if (xml.empty() || !res) {
    return !required;
  }
  std::string str(xml.child_value());
  *res = std::move(str);
  return true;
}

bool miscs_xml_geo2llh(const pugi::xml_node &xml_geo,
                       v2xpb::asn::Position_LLH *position_llh) {
  if (xml_geo.empty() || !position_llh) {
    LOG(ERROR) << "INVALID PARAMS";
    return false;
  }
  if (xml_geo.child("latitude").empty() || xml_geo.child("longitude").empty()) {
    LOG(INFO) << "Missing latlon in xml";
    return false;
  }
  double lat = NAN, lon = NAN;
  if (!miscs_xml_node2double(xml_geo.child("latitude"), &lat, true) ||
      !miscs_xml_node2double(xml_geo.child("longitude"), &lon, true)) {
    LOG(INFO) << "Failed to parse latlon";
    return false;
  }
  position_llh->set_latitude(lat);
  position_llh->set_longitude(lon);
  if (!xml_geo.child("height").empty()) {
    double height = NAN;
    if (miscs_xml_node2double(xml_geo.child("longitude"), &lon, true)) {
      position_llh->set_elevation(height);
    }
  }
  return true;
}

bool map_connection_xml2asn(const pugi::xml_node &movement_xml,
                            Connection_t *connection_asn) {
  if (!connection_asn) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }
  // =================================================================================================================
  int64_t remote_region_node_id = 0;
  if (!miscs_xml_node2long(movement_xml.child("remote_region_node_id"),
                           &remote_region_node_id, true)) {
    LOG(INFO) << "Failed to get connection remote region node id";
    return false;
  }
  connection_asn->remoteIntersection.region =
      ASN1(RoadRegulatorID)::create_tp();
  *connection_asn->remoteIntersection.region = remote_region_node_id / 100000;
  connection_asn->remoteIntersection.id = remote_region_node_id % 100000;
  // =================================================================================================================

  if (!movement_xml.child("connectingLane").empty()) {
    connection_asn->connectingLane = ASN1(ConnectingLane)::create_tp();
    int64_t conn_lane_id = 0;
    if (!miscs_xml_node2long(movement_xml.child("connectingLane").child("lane"),
                             &conn_lane_id, true)) {
      LOG(INFO) << "Failed to get connection lane id";
      return false;
    }
    connection_asn->connectingLane->lane = conn_lane_id;
    connection_asn->connectingLane->maneuver =
        ASN1(AllowedManeuvers)::create_tp();

    ASN1_HOLDER(AllowedManeuvers)
    maneuvers_holder(connection_asn->connectingLane->maneuver);
    maneuvers_holder.init();
    std::string maneuvers_hex;
    if (!miscs_xml_node2string(
            movement_xml.child("connectingLane").child("maneuver"),
            &maneuvers_hex, true)) {
      LOG(INFO) << "Failed to get connection maneuvers";
      return false;
    }
    int64_t maneuvers;
    std::stringstream(maneuvers_hex) >> std::hex >> maneuvers;
    for (int i = 0; i < 16; i++) {
      maneuvers_holder.set_bit(i, maneuvers & 0x8000);
      maneuvers <<= 1;
    }
  }
  // =================================================================================================================
  int64_t phase_id = 0;
  if (!miscs_xml_node2long(movement_xml.child("phase_id"), &phase_id, true)) {
    LOG(INFO) << "Failed to get connection phase id";
    return false;
  }
  connection_asn->phaseId = ASN1(PhaseID)::create_tp();
  *connection_asn->phaseId = phase_id;
  // =================================================================================================================
  ASN1(Connection)::encode_str(ATS_UNALIGNED_BASIC_PER, connection_asn);
  return true;
}

bool map_lane_xml2asn(const pugi::xml_node &lane_xml, int64_t lane_id,
                      int64_t lane_width, Lane_t *lane_asn) {
  if (!lane_asn) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }
  lane_asn->laneID = lane_id;
#if defined(CONFIG_ADAPTER_MODULE_asn_3_ENABLE) || \
    defined(CONFIG_ADAPTER_MODULE_asn_2_ENABLE)
  lane_asn->laneWidth = ASN1(LaneWidth)::create_tp();
  *lane_asn->laneWidth = lane_width;
#endif
  // =================================================================================================================
  lane_asn->laneAttributes = ASN1(LaneAttributes)::create_tp();
  lane_asn->laneAttributes->laneType.present = LaneTypeAttributes_PR_vehicle;
  lane_asn->laneAttributes->shareWith = ASN1(LaneSharing)::create_tp();
  // =================================================================================================================
  lane_asn->maneuvers = ASN1(AllowedManeuvers)::create_tp();

  ASN1_HOLDER(AllowedManeuvers) maneuvers_holder(lane_asn->maneuvers);
  maneuvers_holder.init();
  for (int i = 0; i < 8; i++) {
    auto xml_tag_name =
        std::string("allowed_maneuvers_").append(std::to_string(i));
    const auto &tmp_xml =
        lane_xml.child("lane_base").child(xml_tag_name.c_str());
    if (tmp_xml.empty()) {
      continue;
    }
    int64_t maneuvers = 0;
    if (!miscs_xml_node2long(tmp_xml, &maneuvers, true)) {
      LOG(INFO) << "Failed to get maneuvers";
      return false;
    }
    for (int j = 0; j < 8; j++) {
      maneuvers_holder.set_bit(i * 8 + j, maneuvers & 1);
      maneuvers >>= 1;
    }
  }
  if (!lane_xml.child("movements").empty()) {
    for (const auto movement_xml : lane_xml.child("movements").children()) {
      ASN1(ConnectsToList)::create_tp(&lane_asn->connectsTo);
      ASN_SEQUENCE_ADD(&lane_asn->connectsTo->list,
                       ASN1(Connection)::create_tp());
      if (!map_connection_xml2asn(
              movement_xml, lane_asn->connectsTo->list
                                .array[lane_asn->connectsTo->list.count - 1])) {
        LOG(INFO) << "Failed to convert map connection";
        return false;
      }
    }
  }
  // =================================================================================================================
  if (!lane_xml.child("points").empty()) {
    for (const auto &point_xml : lane_xml.child("points").children()) {
      ASN1(PointList)::create_tp(&lane_asn->points);
      ASN_SEQUENCE_ADD(&lane_asn->points->list, ASN1(RoadPoint)::create_tp());
      auto *point_asn =
          lane_asn->points->list.array[lane_asn->points->list.count - 1];
      v2xpb::asn::Position position;
      if (!miscs_xml_geo2llh(point_xml.child("geo"), position.mutable_llh())) {
        LOG(INFO) << "Failed to convert geo";
        continue;
      }
      if (!position_pb2asn(nullptr, position, &point_asn->posOffset)) {
        LOG(INFO) << "Failed to convert position";
        continue;
      }
    }
  }
  if (lane_asn->points->list.count < 2) {
    LOG(INFO) << "Point size is illegal";
    return false;
  }
  // =================================================================================================================
  ASN1(Lane)::encode_str(ATS_UNALIGNED_BASIC_PER, lane_asn);
  return true;
}

bool map_link_xml2asn(const pugi::xml_node &link_xml, Link_t *link_asn) {
  if (!link_asn) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }
  // =================================================================================================================
  std::string link_name;
  if (!miscs_xml_node2string(link_xml.child("link_base").child("name"),
                             &link_name)) {
    LOG(INFO) << "Failed to get link name";
    return false;
  }
  link_asn->name = ASN1(DescriptiveName)::create_tp();
  OCTET_STRING_fromBuf(link_asn->name, link_name.c_str(),
                       static_cast<int>(link_name.length()));

  int64_t link_region_node_id = 0;
  if (!miscs_xml_node2long(
          link_xml.child("link_base").child("upstr_region_node_id"),
          &link_region_node_id, true)) {
    LOG(INFO) << "Failed to get link upstream region node id";
    return false;
  }
  link_asn->upstreamNodeId.region = ASN1(RoadRegulatorID)::create_tp();
  *link_asn->upstreamNodeId.region = link_region_node_id / 100000;
  link_asn->upstreamNodeId.id = link_region_node_id % 100000;
  // =================================================================================================================
  int64_t link_width = 0;
  if (!miscs_xml_node2long(link_xml.child("link_base").child("link_width"),
                           &link_width, true)) {
    LOG(INFO) << "Failed to get link width";
    return false;
  }
#if defined(CONFIG_ADAPTER_MODULE_asn_3_ENABLE)
  link_asn->linkWidth = link_width;
#elif defined(CONFIG_ADAPTER_MODULE_asn_2_ENABLE)
  link_asn->linkWidth = ASN1(LaneWidth)::create_tp();
  *link_asn->linkWidth = link_width;
#elif defined(CONFIG_ADAPTER_MODULE_asn_1_ENABLE)
  link_asn->laneWidth = 350;
#endif
  // =================================================================================================================
  int64_t speed_limit_km_h = 0;
  if (!link_xml.child("link_base").child("default_max_speed_limit").empty()) {
    if (!miscs_xml_node2long(
            link_xml.child("link_base").child("default_max_speed_limit"),
            &speed_limit_km_h)) {
      LOG(INFO) << "Failed to get default_max_speed_limit";
      return false;
    }
    ASN1(SpeedLimitList)::create_tp(&link_asn->speedLimits);
    ASN_SEQUENCE_ADD(&link_asn->speedLimits->list,
                     ASN1(RegulatorySpeedLimit)::create_tp());
    auto *speed_limit_asn = link_asn->speedLimits->list
                                .array[link_asn->speedLimits->list.count - 1];
    speed_limit_asn->type = SpeedLimitType_vehicleMaxSpeed;
    double slkmh = speed_limit_km_h;
    speed_limit_asn->speed =
        (int64_t)floor(slkmh / 3.6 * 50);  // Unit: 0.02 m/s
  }
  // =================================================================================================================
  if (link_xml.child("lanes").empty()) {
    LOG(INFO) << "No lanes in link!";
    return false;
  }
  int64_t lane_id = 1;
  int64_t lane_width = 0;
  if (!miscs_xml_node2long(link_xml.child("link_base").child("lane_width"),
                           &lane_width, true)) {
    LOG(INFO) << "Failed to get link lane width";
    return false;
  }
  for (const auto &lane_xml : link_xml.child("lanes").children()) {
    ASN_SEQUENCE_ADD(&link_asn->lanes.list, ASN1(Lane)::create_tp());
    if (!map_lane_xml2asn(
            lane_xml, lane_id++, lane_width,  //
            link_asn->lanes.list.array[link_asn->lanes.list.count - 1])) {
      LOG(INFO) << "Failed to convert map lane";
      return false;
    }
  }
  if (link_asn->lanes.list.count > 0) {
    lane_id = (link_asn->lanes.list.count - 1) / 2;
    auto *lane_center = link_asn->lanes.list.array[lane_id];
    if (lane_center->points) {
      auto points_str = ASN1(PointList)::encode_str(ATS_UNALIGNED_BASIC_PER,
                                                    lane_center->points);
      link_asn->points = ASN1(PointList)::decode_tp(
          ATS_UNALIGNED_BASIC_PER, points_str.c_str(), points_str.length());
    }
  }
  // =================================================================================================================
  ASN1(Link)::encode_str(ATS_UNALIGNED_BASIC_PER, link_asn);
  return true;
}

bool map_node_xml2asn(const pugi::xml_node &node_xml, Node_t *node_asn) {
  if (!node_asn) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }
  std::string node_name;
  if (!miscs_xml_node2string(node_xml.child("node_base").child("name"),
                             &node_name)) {
    LOG(INFO) << "Failed to get node name";
    return false;
  }
  node_asn->name = ASN1(DescriptiveName)::create_tp();
  OCTET_STRING_fromBuf(node_asn->name, node_name.c_str(),
                       static_cast<int>(node_name.length()));
  // =================================================================================================================
  int64_t region_node_id = 0;
  if (!miscs_xml_node2long(node_xml.child("node_base").child("region_node_id"),
                           &region_node_id, true)) {
    LOG(INFO) << "Failed to get region node id";
    return false;
  }
  node_asn->id.region = ASN1(RoadRegulatorID)::create_tp();
  *node_asn->id.region = region_node_id / 100000;
  node_asn->id.id = region_node_id % 100000;
  // =================================================================================================================
  v2xpb::asn::Position position;
  if (!miscs_xml_geo2llh(node_xml.child("node_base").child("geo"),
                         position.mutable_llh())) {
    LOG(INFO) << "Failed to convert geo to llh";
    return false;
  }
  if (!position_pb2asn(position, &node_asn->refPos)) {
    LOG(INFO) << "Failed to convert position";
    return false;
  }
  // =================================================================================================================
  ASN1(Node)::encode_str(ATS_UNALIGNED_BASIC_PER, node_asn);
  // =================================================================================================================
  if (node_xml.child("links").empty()) {
    return true;
  }
  for (const auto &link_xml : node_xml.child("links").children()) {
    ASN1(LinkList)::create_tp(&node_asn->inLinks);
    ASN_SEQUENCE_ADD(&node_asn->inLinks->list, ASN1(Link)::create_tp());
    if (!map_link_xml2asn(
            link_xml,
            node_asn->inLinks->list.array[node_asn->inLinks->list.count - 1])) {
      LOG(INFO) << "Failed to convert map link";
      return false;
    }
  }
  // =================================================================================================================
  ASN1(Node)::encode_str(ATS_UNALIGNED_BASIC_PER, node_asn);
  return true;
}

bool map_xml2asn(const pugi::xml_document &map_xml, MapData_t *map_asn) {
  if (!map_asn) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }
  map_asn->msgCnt = 1;
  //    map_asn->timeStamp = ASN1(MinuteOfTheYear)::create_tp();
  //    *map_asn->timeStamp = 0;
  if (map_xml.child("nodes").empty()) {
    LOG(INFO) << "No nodes in xml";
    return false;
  }
  for (const auto &node_xml : map_xml.child("nodes").children()) {
    ASN_SEQUENCE_ADD(&map_asn->nodes.list, ASN1(Node)::create_tp());
    if (!map_node_xml2asn(
            node_xml,
            map_asn->nodes.list.array[map_asn->nodes.list.count - 1])) {
      LOG(INFO) << "Failed to convert map node";
      return false;
    }
  }
  // DEBUG
  ASN1(NodeList)::encode_str(ATS_UNALIGNED_BASIC_PER, &map_asn->nodes);
  return true;
}
